/**
    Copyright 2015 Table Rider
    By: Richard Solti
*/

#include "CordSystem.h"
#include <math.h>
#include "SerialCom.h"
#include "include/tblrdr_cmds.h"
#include <stdlib.h>
#include <stdio.h>
#include <cstring>
#include <list>

#include <iostream>

#define PI    3.14159265358979323846
#define PPM  98.39979575915521438631
#define PPD 113.34842767295597484276
/////////////////////////////////////////////////////////////////////////////////////////
inline double distance(long x1, long y1, long x2, long y2)
{
    return sqrt(pow(x1-x2,2)+pow(y1-y2,2));    /**< Calculates the distance between two points */
}
inline double toRad(double a)
{
    return a * (PI / 180.0);    /**< Converts Degrees to Radians */
}
inline double toDeg(double a)
{
    return a * (180 / PI);    /**< Converts Radians to Degrees */
}
/////////////////////////////////////////////////////////////////////////////////////////
CordSystem::CordSystem(double xValue, double yValue, SerialCom * serialcom)
{
    sercom = serialcom;
    posX = xValue;
    posY = yValue;
    angle = 0;
    sEncL = 0;
    sEncR = 0;
    gEncL = 0;
    sEncL = 0;
    gEncR = 0;
    sEncR = 0;
    diffL = 0;
    diffR = 0;
}

void CordSystem::goToJohnPos(double xValue, double yValue)
{
    double sourceX, sourceY, distins, toX, toY, dir;
    if(posXlist.empty())
    {
        sourceX = posX;
        sourceY = posY;
    }
    else
    {
        sourceX = posXlist.back();
        sourceY = posYlist.back();
    }
    distins = distance(sourceX, sourceY, xValue, yValue);
    if(distins < 25000 / PPM)//John must be at least 25k pulses away from doe
        return;
    distins = distins - 20000 / PPM;
    dir = atan2(yValue - sourceY, xValue - sourceX);
    toX = cos(dir) * distins;
    toY = sin(dir) * distins;
    moveBy(toX, toY);//Redigerad direkt i google-code
}

/** \brief Constructor
 *
 * \param xValue double The starting x-position
 * \param yValue double The starting y-position
 *
 */
CordSystem::CordSystem(double xValue, double yValue)
{
    posX = xValue;
    posY = yValue;
}

/** \brief Returns the current X-position of the robot (Takes mid-step position into consideration)
 *
 * \return double The X-position
 *
 */
double CordSystem::getPosX()
{
    updateSerialEnc();
    return posX; //- diffL == 0 ? 0 : (((double)(currEncL - sEncL) / diffL) / PPM) * cos(toRad(angle));
}

/** \brief Returns the current Y-position of the robot (Note, this takes mid-step position into consideration)
 *
 * \return double The Y-position
 *
 */
double CordSystem::getPosY()
{
    updateSerialEnc();
    return posY; //- diffR == 0 ? 0 : (((double)(currEncR - sEncR) / diffR) / PPM) * sin(toRad(angle));;
}

void CordSystem::updateSerialEnc()
{
    int k=0,j=0;
    short code = TBLRDR_GETENC;
    const char * buff = sercom->send_msg((void*)&code);
    //printf("%s\n", buff);
    while(buff[k] != '\n' && buff[k] != '\255'){k++;}
    char* arg1 = (char*)malloc((k-2)*sizeof(char));
    //printf("Allocated array with %d spaces.\n", k-2);
    //printf("0x%lx\n", (unsigned long)arg1);
    //printf("Copying data starting with %c...\n", buff[2]);
    memcpy(arg1, buff+2, k - 2);
    currEncL = atoi(arg1);
    free(arg1);
    k += 3;
    while(buff[k] != '\n'){ k++; j++;}
    arg1 = (char*)malloc(j*sizeof(char));
    memcpy(arg1, buff+k-j, j);
    currEncR = atoi(arg1);
    free(arg1);
}


/** \brief Returns the current direction the robot is facing. (Note, this does not take mid-step into consideration)
 *
 * \return A double that represent the direcion in degrees
 *
 */
double CordSystem::getDirection()
{
    return angle;
}

int CordSystem::getStepAmount()
{
    return stepType.size();
}

void CordSystem::setPosX(double d)
{
	posX = d;
}
void CordSystem::setPosY(double d)
{
	posY = d;
}

/** \brief Sets the variable "angle" to the given value. This should only be used at startup.
 *
 * \param degrees double The value to set angle to
 * \return void
 *
 */
void CordSystem::setDirection(double degrees)
{
    angle = degrees;
}

void CordSystem::sendToController(long leftValue, long rightValue)
{
    char msg[10];
    short code1 = TBLRDR_CONTROLINIT;
    memcpy(msg, (char*)&code1, 2);
    memcpy(msg + 2, (char*)&leftValue, 4);
    memcpy(msg + 6, (char*)&rightValue, 4);
    sercom->send_msg(msg);
}


/** \brief Initialises the controller by looking up what the next step is and doing it. The next step can only be done if the controller is done. Calling it before the controller is done will have no effect!
 *
 * \return void
 *
 */
void CordSystem::doNextStep()
{
    short code = TBLRDR_GETCONTROLDONE;
    const char* tmp = sercom->send_msg((void *)&code);
    int initdone = (int)*tmp;
    if(!stepType.empty() && initdone == '1')   //control->done())
    {
        if(stepType.front() == 'r')
        {
            updateSerialEnc();
            double lengthToRoll = distance(posX, posY, posXlist.front(), posYlist.front());
            sEncL = currEncL;/**< These values (enc to diff) are used for calculating the "exact approximate" positon. (Which is basically where the robot is between two steps) */
            sEncR = currEncR;
            gEncL = sEncL + lengthToRoll * PPM;
            gEncR = sEncR + lengthToRoll * PPM;
            diffL = gEncL - sEncL;
            diffR = gEncR - sEncR;
            sendToController((long)(lengthToRoll * PPM), (long)(lengthToRoll * PPM));
            posX = posXlist.front();
            posY = posYlist.front();
            posXlist.pop_front();
            posYlist.pop_front();
        }
        else if(stepType.front() == 't')
        {
            double turnto = turnList.front();
            double tx1 = cos(toRad(angle)), ty1 = sin(toRad(angle)),
                   tx2 = cos(toRad(turnto)), ty2 = sin(toRad(turnto));
            double crossProduct = tx1 * ty2 - tx2 * ty1; /**< Calculate cross product to determine left or right turning */
            if(crossProduct > 0)
            {
                double toturn = (360 - angle) + turnto;/**< Calculate how much to turn by */
                if(toturn > 360)
                    toturn -= 360;
                sendToController((long)(-toturn * PPD), (long)((toturn) * PPD));/**< Send information to PI-controller */
            }
            else if(crossProduct < 0)
            {
                double toturn = (360 - turnto) + angle;
                if(toturn > 360)
                    toturn -= 360;
                sendToController((long)(toturn * PPD), (long)(-toturn * PPD));
            }
            else
            {
                if(turnto > angle)
                    sendToController((long)(-(turnto - angle) * PPD), (long)((turnto - angle) * PPD));
                else if(angle > turnto)
                    sendToController((long)((angle - turnto) * PPD), (long)(-(angle - turnto) * PPD));
                else
                    {}
            }
            angle = turnto;
            sEncL = 0;
            sEncR = 0;
            gEncL = 0;
            sEncL = 0;
            gEncR = 0;
            sEncR = 0;
            diffL = 0;
            diffR = 0; /**< Robots position does not change, only angle*/
            turnList.pop_front();
        }
        stepType.pop_front();
    }
}

/** \brief The robot turns left/right depending on the sign of the given value
 *
 * \param degrees double The given value in degrees to turn.
 * \return void
 *
 */
void CordSystem::turnBy(double degrees)
{
    double lastAngle, wantedAngle;
    if(turnList.empty())
        lastAngle = angle;
    else
        lastAngle = turnList.back();
    wantedAngle = degrees + lastAngle;
    if(wantedAngle > 360)
        wantedAngle -= 360;
    else if(wantedAngle < 0)
        wantedAngle += 360;
    turnList.push_back((wantedAngle == 360) ? 0 : wantedAngle);
    stepType.push_back('t');
}

/** \brief Turns the robot to the given angle
 *
 * \param degrees double The angle to turn to
 * \return void
 *
 */
void CordSystem::turnTo(double degrees)
{
    while(degrees < 0)
        degrees+=360;
    while(degrees > 360)
        degrees-=360;
//    if(lastangle != degrees)
//    {
    turnList.push_back((degrees == 360) ? 0 : degrees);
    stepType.push_back('t');
//    }
}

/** \brief Moves the robot by the given vector
 *
 * \param xValue double The x-component of the vector
 * \param yValue double The y-component of the vector
 * \return void
 *
 */
void CordSystem::moveBy(double xValue, double yValue)
{
    double sourceX, sourceY;
    if(posXlist.empty())
    {
        sourceX = posX;
        sourceY = posY;
    }
    else
    {
        sourceX = posXlist.back();
        sourceY = posYlist.back();
    }
    moveTo(sourceX + xValue, sourceY + yValue);
}


/** \brief Moves the robot to the given coordinates
 *
 * \param xValue double The x-component of the coordinate
 * \param yValue double The y-component of the coordinate
 * \return void
 *
 */
void CordSystem::moveTo(double xValue, double yValue)
{
    double sourceX, sourceY;
    if(posXlist.empty())   //Find the correct origin to us
    {
        sourceX = posX;
        sourceY = posY;
    }
    else
    {
        sourceX = posXlist.back();
        sourceY = posYlist.back();
    }

    if(xValue != sourceX || yValue != sourceY)
    {
        turnTo(toDeg(atan2(yValue - sourceY, xValue - sourceX))); //Calculate which direction to face when moving to (xValue, yValue)
        posXlist.push_back(xValue); //Then add a forward movement step
        posYlist.push_back(yValue);
        stepType.push_back('r');
    }
}

/** \brief Makes the robot move forward
 *
 * \param length double How much the robot will move forward.
 * \return void
 *
 */
void CordSystem::moveStraight(double length)
{
    if(length<=0)
        return;
    double sourceAngle;
    if(turnList.empty())                    //Find the correct angle to use
        sourceAngle = angle;
    else
        sourceAngle = turnList.back();

    //Calculate how much to move on x- and y-axis, then send the resulting 'offset' to moveBy
    moveBy(cos(toRad(sourceAngle)) * length, sin(toRad(sourceAngle)) * length);
}
void CordSystem::update()
{

}
char CordSystem::getNextStepType()
{
    return stepType.front();
}

double * CordSystem::getNextStepValue()
{
    if(stepType.front() == 't')
    {
        static double aaa[1];
        aaa[0] = turnList.front();
        return aaa;
    }
    else
    {
        static double bbb[2];
        bbb[0] = posXlist.front();
        bbb[1] = posYlist.front();
        return bbb;
    }
}
